import sys
try:
    sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
except:
    pass
import cv2 #导入opencv包
import numpy as np #导入numpy 取别名np

# # 4,6 可打开
# name = "/dev/rgb_camera"

# for index in range(10):
#     # cap = cv2.VideoCapture(name)
#     cap = cv2.VideoCapture(index)
#     print(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
#     if cap.isOpened():
#         for i in range(50):
#             success, image = cap.read()  # 图片读进来是BGR
#             if not success:
#                 print("摄像头被占用！")
#                 break
#         # print(name + str(index) + " open")
#         cv2.imwrite("./rgb_test{}.jpg".format(index),image)
#         # cv2.imwrite("./rgb_test.jpg",image)
#     else:
#         print(name + str(index) + " open failed")
#         # print(" open failed")
# cap.release()

# 4,6 可打开
name = "/dev/video6"

cap = cv2.VideoCapture(name)
print(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
if cap.isOpened():
    for i in range(50):
        success, image = cap.read()  # 图片读进来是BGR
        if not success:
            print("摄像头被占用！")
            break
    # print(name + str(index) + " open")
    cv2.imwrite("./rgb_test.jpg",image)
else:
    print(" open failed")
cap.release()